#include "P18F4455.INC"

EXTERN x, X, y, H, y_last; defined in C module

AFFTF  MACRO  VAR1, VAR2, OUT
	MOVF VAR1, 0
	MULWF (VAR2)
 	BCF  STATUS, C       ; Clear carry
	movf PRODL, 0
	addwf (OUT), 1
	movf PRODH, 0
	addwfc (OUT)+1, 1
	movlw 0x00
	addwfc (OUT)+2, 1
	movlw 0x00
	addwfc (OUT)+3, 1
	ENDM

SFFTF  MACRO  VAR1, VAR2, OUT
	MOVF VAR1, 0
	MULWF (VAR2)
 	BCF  STATUS, C       ; Clear carry
	movf PRODL, 0
	SUBWFB (OUT), 1
	movf PRODH, 0
	SUBWFB (OUT)+1, 1
	movlw 0x00
	SUBWFB (OUT)+2, 1
	movlw 0x00
	SUBWFB (OUT)+3, 1
	ENDM

LSR16   MACRO   VAR16
        BCF     STATUS, C       ; Clear carry
        RRCF    (VAR16)+1,F     ; Rotate high byte right
        RRCF    (VAR16),F       ; Rotate low byte right
        ENDM



CODE
asm_iir
;Start Conversie
	BSF ADCON0, GO

;X -=  (unsigned int)x[0] * 128;
	MOVLW D'128'
	MULWF x
 	BSF  STATUS, C       ; Clear carry
	MOVF PRODL, 0
	SUBWFB X, 	1
	MOVF PRODH, 0
	SUBWFB X+1, 1
	MOVLW 0x00
	SUBWFB X+2, 1
	MOVLW 0x00
	SUBWFB X+3, 1

;X +=  (unsigned int)y_last[0] * 128;
	MOVLW D'128'
	MULWF y_last
 	BCF  STATUS, C       ; Clear carry
	MOVF PRODL, 0
	ADDWFC X, 	1
	MOVF PRODH, 0
	ADDWFC X+1, 1
	MOVLW 0x00
	ADDWFC X+2, 1
	MOVLW 0x00
	ADDWFC X+3, 1

;Mutam elementele lui x la stanga cu o pozitie
	MOVFF x+1,x
	MOVFF x+2,x+1
;Mutam elementele lui y_last la stanga cu o pozitie
	MOVFF y_last+1,y_last

;y_last[1] = y
	MOVFF y, y_last+1

;y = 16384; 128 * 128
   CLRF y
   MOVLW 0x40
   MOVWF y+1
   CLRF y+2
   CLRF y+3

;Asteapta conversie
	TEST_CONV:
	BTFSC	ADCON0, GO
	BRA		TEST_CONV

;debug
;	MOVLW 0xFF
;	MOVWF ADRESL

;divide by 4 (result >> 2
	LSR16 ADRESL	
	LSR16 ADRESL

;x[2] = result;
	MOVFF ADRESL, x+2

;y += (unsigned int)h[0] * (unsigned int)x[2];
   CLRF FSR0H
   MOVFF H, FSR0L
   AFFTF POSTINC0 , x+2, y
;y += (unsigned int)h[1] * (unsigned int)x[1]; 	
   AFFTF POSTINC0, x+1, y
;y += (unsigned int)h[2] * (unsigned int)x[0];
   AFFTF POSTINC0, x, y
;y += (unsigned int)h[3] * (unsigned int)y_last[1];
   SFFTF POSTINC0, y_last+1, y
;y += (unsigned int)h[4] * (unsigned int)y_last[0];
   SFFTF POSTINC0, y_last, y

;X +=  (unsigned int)x[2] * 128;
	MOVLW D'128'
	MULWF x+2
 	BCF  STATUS, C       ; Clear carry
	MOVF PRODL, 0
	ADDWFC X, 	1
	MOVF PRODH, 0
	ADDWFC X+1, 1
	MOVLW 0x00
	ADDWFC X+2, 1
	MOVLW 0x00
	ADDWFC X+3, 1

;X -=  (unsigned int)ylast[1] * 128;
	MOVLW D'128'
	MULWF y_last+1
 	BSF  STATUS, C       ; Clear carry
	MOVF PRODL, 0
	SUBWFB X, 	1
	MOVF PRODH, 0
	SUBWFB X+1, 1
	MOVLW 0x00
	SUBWFB X+2, 1
	MOVLW 0x00
	SUBWFB X+3, 1



;y -= X;   
   BSF  	STATUS, C       ; Clear carry
   MOVF		X, 0
   SUBWFB	y, 1
   MOVF   	X+1, 0
   SUBWFB 	y+1, 1
   MOVF 	X+2, 0
   SUBWFB 	y+2, 1
   MOVF 	X+3, 0
   SUBWFB 	y+3, 1

; y /= 128; (y >> 7)
	LSR16 y
	LSR16 y
	LSR16 y
	LSR16 y
	LSR16 y
	LSR16 y
	LSR16 y

; SetDCPWM1(y);
	MOVFF 	y,	CCPR1L
	BCF		CCP1CON, 4
	BCF		CCP1CON, 5

	RETURN
;end asm_fir

; export so linker can see it
GLOBAL asm_iir
END